#include "interface_can.hpp"


#include "freertos_vars.h"
#include "utils.hpp"

#include <can.h>
#include <cmsis_os.h>


// Specific CAN Protocols
#include "can_simple.hpp"

// Safer context handling via maps instead of arrays
// #include <unordered_map>
// std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;

// Constructor is called by communication.cpp and the handle is assigned appropriately
ODriveCAN::ODriveCAN(ODriveCAN::Config_t &config, uint32_t handle)
    : config_{config},
      handle_{handle} {
    // ctxMap[handle_] = this;
}

void ODriveCAN::can_server_thread() {
    for (;;) {
        can_error_enum status = can_error_get(handle_);
        if (status == CAN_ERROR_NONE) {
            can_Message_t rxmsg;
            osSemaphoreWait(sem_can, 10);  // Poll every 10ms regardless of sempahore status
            while (available()) {
                read(rxmsg);
                
                switch (config_.protocol) {
                    case PROTOCOL_SIMPLE:
                        CANSimple::handle_can_message(rxmsg);
                        break;
                }
            }
            can_interrupt_enable(CAN0,CAN_INT_RFNE0);
        }
    }
}

static void can_server_thread_wrapper(void *ctx) {
    reinterpret_cast<ODriveCAN *>(ctx)->can_server_thread();
    reinterpret_cast<ODriveCAN *>(ctx)->thread_id_valid_ = false;
}

bool ODriveCAN::start_can_server() {
    can_interrupt_enable(CAN0,CAN_INT_RFNE0);
    osThreadDef(can_server_thread_def, can_server_thread_wrapper, osPriorityNormal, 0, stack_size_ / sizeof(StackType_t));
    thread_id_ = osThreadCreate(osThread(can_server_thread_def), this);
    thread_id_valid_ = true;
    return true;
}

// Send a CAN message on the bus
uint32_t ODriveCAN::write(can_Message_t &txmsg) {
    if (can_error_get(handle_) == CAN_ERROR_NONE) {
        can_trasnmit_message_struct header;
        header.tx_sfid = txmsg.id;
        header.tx_efid = txmsg.id;
        header.tx_ff = txmsg.isExt ? CAN_FF_EXTENDED : CAN_FF_STANDARD;
        header.tx_ft = CAN_FT_DATA;
        header.tx_dlen = txmsg.len;
        uint32_t retTxMailbox = 0;
        for(int i =0;i<8;i++)
            header.tx_data[i] = txmsg.buf[i];
        retTxMailbox = can_message_transmit(handle_,&header);
        return retTxMailbox;
    } else {
        return -1;
    }
}

uint32_t ODriveCAN::available() {
    return (can_receive_message_length_get(handle_, CAN_FIFO0));
}

bool ODriveCAN::read(can_Message_t &rxmsg) {
    can_receive_message_struct header;
    bool validRead = false;
    if (can_receive_message_length_get(handle_, CAN_FIFO0) > 0) {
        can_message_receive(handle_, CAN_FIFO0, &header);
        validRead = true;
    } 
    rxmsg.isExt = header.rx_ff;
    rxmsg.id = rxmsg.isExt ? header.rx_efid : header.rx_sfid;  // If it's an extended message, pass the extended ID
    rxmsg.len = header.rx_dlen;
    rxmsg.rtr = header.rx_ft;
    memcpy(rxmsg.buf,header.rx_data,(size_t)header.rx_dlen);
    return validRead;
}

// Set one of only a few common baud rates.  CAN doesn't do arbitrary baud rates well due to the time-quanta issue.
// 21 TQ allows for easy sampling at exactly 80% (recommended by Vector Informatik GmbH for high reliability systems)
// Conveniently, the CAN peripheral's 42MHz clock lets us easily create 21TQs for all common baud rates

void ODriveCAN::reinit_can() {
    /* HAL_CAN_Stop(handle_);
    HAL_CAN_Init(handle_);
    auto status = HAL_CAN_Start(handle_);
    if (status == HAL_OK)
        status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING); */
}

void ODriveCAN::set_error(Error error) {
    error_ = error;
}
// This function is called by each axis.
// It provides an abstraction from the specific CAN protocol in use
void ODriveCAN::send_heartbeat(Axis *axis) {
    // Handle heartbeat message
    if (axis->config_.can_heartbeat_rate_ms > 0) {
        uint32_t now = osKernelSysTick();    /* ms�� ��ʱ��*/
        if ((now - axis->last_heartbeat_) >= axis->config_.can_heartbeat_rate_ms) {
            switch (config_.protocol) {
                case PROTOCOL_SIMPLE:
                    CANSimple::send_heartbeat(axis);   
                    break;
            }
            axis->last_heartbeat_ = now;
        }
    }
}

void ODriveCAN::send_encoder_count(Axis *axis) {

        uint32_t now = osKernelSysTick();
		if (axis->config_.can_encoder_count_rate_ms > 0) {
			
            if ((now - axis->last_encoder_count_) >= axis->config_.can_encoder_count_rate_ms) {  //20ms
			
  			    //CANSimple::active_upload_encoder_count(axis,axis->encoder_count_times_);  //updata
                //printf("A%f\r\n",axes[1]->motor_.current_meas_.phB);
                //printf("B%f\r\n",axes[1]->motor_.current_meas_.phC);
		        axis->encoder_count_times_++;              //0~180000
                if(axis->encoder_count_times_ > 180000U)
				    axis->encoder_count_times_ = 1U;
		
                axis->last_encoder_count_ = now;	
            }
        }

}
void ODriveCAN::send_error (Axis *axis) {		

	uint32_t now = osKernelSysTick();
	
	if (axis->config_.can_updata_error_ms > 0) 
    {
	    if ((now - axis->last_updata_error_) >= axis->config_.can_updata_error_ms) {
		
		    CANSimple::active_upload_error(axis);	
		    axis->last_updata_error_ = now;

			axis->flag_axis_en = false;
	    }
	}
}
